{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "d56d9b7b",
   "metadata": {},
   "source": [
    "# SE-Sync benchmarking"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2bc913e7",
   "metadata": {},
   "source": [
    "This notebook uses SE-Sync's Python bindings to permit easy benchmarking against a collection of large-scale SLAM benchmarks"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cb95dfad",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "import numpy as np\n",
    "import time\n",
    "\n",
    "import pandas as pd\n",
    "from matplotlib import pyplot as plt\n",
    "import seaborn as sns\n",
    "from IPython.display import display  # To get pretty tabular output w/ PANDAS\n",
    "import pickle\n",
    "\n",
    "import PySESync"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bcaf11ef",
   "metadata": {},
   "source": [
    "### Test configuration"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "80851b69",
   "metadata": {},
   "source": [
    "#### Test benchmarks"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b184695",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Location of folder containing the benchmarks\n",
    "data_folder = \"/home/dmrosen/SE-Sync/data/\"\n",
    "\n",
    "# Specific list of files to process\n",
    "files = [\"manhattan\", \"city10000\", \"MIT\", \"CSAIL\", \"intel\", \"sphere2500\", \"torus3D\", \"parking-garage\", \"rim\"]\n",
    "#files = [\"sphere2500\"]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7e589774",
   "metadata": {},
   "source": [
    "#### SE-Sync configurations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7ffafbf",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set list of SE-Sync configurations to process\n",
    "\n",
    "# Common configurations\n",
    "num_threads = 4\n",
    "verbose = False\n",
    "\n",
    "opts_list = [PySESync.SESyncOpts() for i in range(6)]\n",
    "\n",
    "# Config 0: Simplified w/ chordal init\n",
    "opts_list[0].formulation = PySESync.Formulation.Simplified\n",
    "opts_list[0].initialization = PySESync.Initialization.Chordal\n",
    "opts_list[0].num_threads = 4\n",
    "opts_list[0].verbose = verbose\n",
    "\n",
    "# Config 1: Simplified w/ random init\n",
    "opts_list[1].formulation = PySESync.Formulation.Simplified\n",
    "opts_list[1].initialization = PySESync.Initialization.Random\n",
    "opts_list[1].num_threads = 4\n",
    "opts_list[1].verbose = verbose\n",
    "\n",
    "# Config 2: Explicit w/ chordal init\n",
    "opts_list[2].formulation = PySESync.Formulation.Explicit\n",
    "opts_list[2].initialization = PySESync.Initialization.Chordal\n",
    "opts_list[2].num_threads = 4\n",
    "opts_list[2].verbose = verbose\n",
    "\n",
    "# Config 3: Explicit w/ random init\n",
    "opts_list[3].formulation = PySESync.Formulation.Explicit\n",
    "opts_list[3].initialization = PySESync.Initialization.Random\n",
    "opts_list[3].num_threads = 4\n",
    "opts_list[3].verbose = verbose\n",
    "\n",
    "# Config 4: SOSync w/ chordal init\n",
    "opts_list[4].formulation = PySESync.Formulation.SOSync\n",
    "opts_list[4].initialization = PySESync.Initialization.Chordal\n",
    "opts_list[4].num_threads = 4\n",
    "opts_list[4].verbose = verbose\n",
    "\n",
    "# Config 5: SOSync w/ random init\n",
    "opts_list[5].formulation = PySESync.Formulation.SOSync\n",
    "opts_list[5].initialization = PySESync.Initialization.Random\n",
    "opts_list[5].num_threads = 4\n",
    "opts_list[5].verbose = verbose\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5acec64b",
   "metadata": {},
   "source": [
    "### Run tests!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "12b3055f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Iterate over the list of benchmarks\n",
    "\n",
    "data = []\n",
    "\n",
    "start_time = time.time()\n",
    "for k, f in enumerate(files):\n",
    "    \n",
    "    # Construct complete path to this file\n",
    "    filename = data_folder + f + \".g2o\"\n",
    "\n",
    "    # Read measurements out of this file\n",
    "    measurements, num_poses = PySESync.read_g2o_file(filename)\n",
    "    \n",
    "    # Get dimension of problem\n",
    "    d = measurements[0].R.shape[0]\n",
    "    \n",
    "    # For each configuration in the test set \n",
    "    for c, opts in enumerate(opts_list):\n",
    "        \n",
    "        # Set initial relaxation rank to be the dimension of this test problem\n",
    "        opts.r0 = d\n",
    "        \n",
    "        # Run SE-Sync with this configuration!\n",
    "        result = PySESync.SESync(measurements, opts)\n",
    "        \n",
    "        # Record results for this test\n",
    "        data.append({\"Config\": c, \"Dataset\" : f, \"Status\" : result.status.name,  \\\n",
    "                    \"Fxhat\" : result.Fxhat, \"GradNorm\" : result.gradnorm, \\\n",
    "                    \"SubOptBound\" : result.suboptimality_bound, \\\n",
    "                    \"TotalTime\" : result.total_computation_time, \\\n",
    "                    #\"InitTime\" : result.initialization_time, \\\n",
    "                    \"OptTime\" : sum(l[-1] for l in result.elapsed_optimization_times), \\\n",
    "                    \"OptIters\" : sum(len(l) for l in result.elapsed_optimization_times), \\\n",
    "                    \"HessVecProds\" : sum(map(sum, result.Hessian_vector_products)), \\\n",
    "                    \"VerTime\" : sum(result.verification_times), \\\n",
    "                    \"VerIters\" : sum(result.LOBPCG_iters), \\\n",
    "                    \"FinalRank\" : result.Yopt.shape[0]})\n",
    "        \n",
    "\n",
    "end_time = time.time()\n",
    "\n",
    "elapsed_time = (end_time - start_time)\n",
    "\n",
    "print(\"All tests finished.  Total computation time: %g seconds\" % elapsed_time)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "80ed155a",
   "metadata": {},
   "source": [
    "### Analysis"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c7d6a519",
   "metadata": {},
   "source": [
    "Create a pandas datafrom from the recorded results for further processing"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13000bc2",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "df = pd.DataFrame(data)\n",
    "display(df)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e258eb7d",
   "metadata": {},
   "source": [
    "Save this data to disk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e327e4cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "df.to_pickle(\"SLAM_benchmarking.pkl\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8e239c1c",
   "metadata": {},
   "source": [
    "Read data from disk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15cc44d9",
   "metadata": {},
   "outputs": [],
   "source": [
    "#df = pd.read_pickle(\"SESync_benchmarking.pkl\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
